// --------------------------------------------------------------------------------------------------------------------
// <copyright file="GlobalMapTest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Navigation.Runtime.UnitTests
{
    using System;
    using System.Collections.Generic;
    using System.Diagnostics;
    using System.Linq;
    using Microsoft.Robotics.Navigation.Localization;
    using Microsoft.Robotics.Navigation.Perception;
    using Microsoft.Robotics.Navigation.Visualization.Localization;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Vision;
    using Microsoft.Robotics.Vision.Cameras;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    /// <summary>
    /// Basic unit test for Global Map
    /// </summary>
    [TestClass]
    public class GlobalMapTest
    {
        /// <summary>
        /// The factor that relates map resolution to precision in surface reconstruction. For 2cm grid cells, we expect 1mm reconstruction precision.
        /// </summary>
        private const int PrecisionFactor = 20;

        /// <summary>
        /// Given a map containing one wall, verifies that ray tracing produces the expected result
        /// This test demonstrates that varying the distance to the obstacle does not introduce any quantization effects 
        /// and despite the coarse resolution of the map (4cm) the obstacle is still retrieved with 1mm accuracy.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void RayTraceWall_MapTest()
        {
            this.RayTraceWall_MapTest(0.02f);
            this.RayTraceWall_MapTest(0.04f);
            this.RayTraceWall_MapTest(0.1f);
            this.RayTraceWall_MapTest(0.2f);
        }

        /// <summary>
        /// Given a map containing one wall, verifies that ray tracing produces the expected result
        /// This test demonstrates that varying the distance to the obstacle does not introduce any quantization effects 
        /// and despite the coarse resolution of the map (4cm) the obstacle is still retrieved with 1mm accuracy.
        /// </summary>
        /// <param name="resolution">The resolution of the map (size of grid cell in meters)</param>
        public void RayTraceWall_MapTest(float resolution)
        {
            Pose2D pose = new Pose2D(0, 750, 0);
            GlobalMap map = new GlobalMap(1500, 1500, pose, resolution);

            // generate a simple map from an HDP - a wall parallel to the camera (y axis) 30m out
            int absoluteWallPosition = 29;
            KinectHorizontalDepthProfile hdp = CreateWallFacingHDP(absoluteWallPosition);
            map.Update(hdp, pose, 2 * absoluteWallPosition, 1);
            ////GlobalMapUtility.ConvertGlobalMapToBitmap(map, @"c:\temp\RayTraceWall_MapTest.png");

            // validate that cell quantization is not a problem by varying the wall distance in small increments (13mm instead of 1mm to speed up the test, result is the same)
            for (double distanceToWall = 20; distanceToWall > 0.5; distanceToWall -= 0.013)
            {
                IHorizontalDepthProfile reprojectedSurface;
                pose = new Pose2D((absoluteWallPosition - distanceToWall) / resolution, 750, 0);

                // generate the reprojected surface (match cost is not important in this test)
                map.Match(hdp, ref pose, true, out reprojectedSurface);

                // there should be no zero points
                // all hit points should be within 1mm of the wall for 0.02 resolution (20-to-1 ratio of resolution to precision)
                int zeroCount = 0;
                int invalidCount = 0;
                double precision = resolution / PrecisionFactor;
                for (int i = 0; i < reprojectedSurface.Length; i++)
                {
                    if (reprojectedSurface.GetHitPoint(i).X == 0)
                    {
                        zeroCount++;
                    }

                    if (Math.Abs(reprojectedSurface.GetHitPoint(i).X - distanceToWall) > precision)
                    {
                        invalidCount++;
                    }
                }

                Assert.AreEqual(0, zeroCount, "Non-zero count of no readings at {0}m for resolution {1}", distanceToWall, resolution);
                Assert.AreEqual(0, invalidCount, "Precision criteria not met at {0}m for resolution {1}", distanceToWall, resolution);
            }
        }

        /// <summary>
        /// Given a map containing one wall, verifies that ray tracing works
        /// This test demonstrates that varying the angle does not introduce any quantization effects 
        /// and despite the coarse resolution of the map (4cm) the obstacle is still retrieved with 1mm accuracy.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void RayTraceTiltedWall_MapTest()
        {
            this.RayTraceTiltedWall_MapTest(0.02f);
            this.RayTraceTiltedWall_MapTest(0.04f);
            this.RayTraceTiltedWall_MapTest(0.1f);
            this.RayTraceTiltedWall_MapTest(0.2f);
        }

        /// <summary>
        /// Given a map containing one wall, verifies that ray tracing works
        /// This test demonstrates that varying the angle does not introduce any quantization effects 
        /// and despite the coarse resolution of the map (4cm) the obstacle is still retrieved with 1mm accuracy.
        /// </summary>
        /// <param name="resolution">The resolution of the map (size of grid cell in meters)</param>
        public void RayTraceTiltedWall_MapTest(float resolution)
        {
            // generate a wall parallel to the camera (y axis) 12m out
            int absoluteWallPosition = 29;
            KinectHorizontalDepthProfile hdp = CreateWallFacingHDP(absoluteWallPosition);

            // validate that cell quantization is not a problem by varying the wall distance and angle in small increments
            for (double angle = Math.PI / 6; angle < Math.PI / 3; angle += MathConstants.Degrees2Radians)
            {
                // draw map with a tilted wall
                GlobalMap map = new GlobalMap(2000, 2000, new Pose2D(), resolution);
                map.Update(hdp, new Pose2D(0, 0, angle), 2 * absoluteWallPosition, 1);
                ////GlobalMapUtility.ConvertGlobalMapToBitmap(map, @"c:\temp\RayTraceTiltedWall_MapTest.png");
                for (double distanceToWall = 10; distanceToWall > 0.6; distanceToWall -= 0.277)
                {
                    IHorizontalDepthProfile reprojectedSurface;
                    double x = ((absoluteWallPosition - distanceToWall) / resolution) * Math.Cos(angle);
                    double y = ((absoluteWallPosition - distanceToWall) / resolution) * Math.Sin(angle);
                    Pose2D pose = new Pose2D(x, y, angle);
                    map.Match(hdp, ref pose, true, out reprojectedSurface);

                    // there should be no zero points
                    // all hit points should be within 1mm of the wall
                    int zeroCount = 0;
                    int invalidCount = 0;
                    for (int i = 0; i < reprojectedSurface.Length; i++)
                    {
                        if (reprojectedSurface.GetHitPoint(i).X == 0)
                        {
                            zeroCount++;
                        }

                        if (Math.Abs(reprojectedSurface.GetHitPoint(i).X - distanceToWall) > 0.001)
                        {
                            invalidCount++;
                        }
                    }

                    Assert.AreEqual(0, zeroCount, "Non-zero count of no readings at {0}m and {1}rad for resolution {2}", distanceToWall, angle, resolution);
                    Assert.AreEqual(0, invalidCount, "Precision criteria not met at {0}m and {1}rad for resolution {2}", distanceToWall, angle, resolution);
                }
            }
        }

        /// <summary>
        /// Given a map containing a corner, verifies that ray tracing produces the expected result.
        /// This test demonstrates that angles are preserved in re-projection.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void RayTraceCorner_MapTest()
        {
            this.RayTraceCorner_MapTest(0.01f);
            this.RayTraceCorner_MapTest(0.04f);
            this.RayTraceCorner_MapTest(0.1f);
            this.RayTraceCorner_MapTest(0.2f);
        }

        /// <summary>
        /// Given a map containing a corner, verifies that ray tracing produces the expected result.
        /// This test demonstrates that angles are preserved in re-projection.
        /// </summary>
        /// <param name="resolution">The resolution of the map (size of grid cell in meters)</param>
        public void RayTraceCorner_MapTest(float resolution)
        {
            int mapSize = 2000;
            Pose2D pose = new Pose2D(0, mapSize / 2, 0);
            GlobalMap map = new GlobalMap(mapSize, mapSize, pose, resolution);

            // generate a map of a corner where two walls intersect (camera is inside the corner)
            int absoluteWallPosition = 11;
            KinectHorizontalDepthProfile hdp = CreateWallFacingHDP(absoluteWallPosition);
            double angle = hdp.FOV / 2;
            map.Update(hdp, new Pose2D(0, mapSize / 2, angle), 2 * absoluteWallPosition, 1);
            map.Update(hdp, new Pose2D(0, mapSize / 2, -angle), 2 * absoluteWallPosition, 1);
           //// GlobalMapUtility.ConvertGlobalMapToBitmap(map, @"c:\temp\RayTraceCorner_MapTest.png");

            // reprojected surface angle should be consistent with the wall angle
            for (double distanceToWall = 10; distanceToWall > 0.5; distanceToWall -= 0.041)
            {
                IHorizontalDepthProfile reprojectedSurface;
                pose = new Pose2D((absoluteWallPosition - distanceToWall) / resolution, mapSize / 2, 0);
                map.Match(hdp, ref pose, true, out reprojectedSurface);

                // there should be no zero points
                int zeroCount = 0;
                for (int i = 0; i < reprojectedSurface.Length; i++)
                {
                    if (reprojectedSurface.GetHitPoint(i).X == 0)
                    {
                        zeroCount++;
                    }
                }

                Assert.AreEqual(0, zeroCount);

                // surface angle should be consistent with the wall angle
                // The epsilon limit comes from the precision of the surface reconstruction
                double precision = resolution / PrecisionFactor;
                double epsilon = Math.Atan2(resolution, 2 * precision);

                // skip the point in the middle where the surface normal is not well defined (because of the way we construct the corner)
                for (int i = 1; i < reprojectedSurface.Length / 2 - 16; i++)
                {
                    Point2D point = reprojectedSurface.GetHitPoint(i);
                    Point2D previousPoint = reprojectedSurface.GetHitPoint(i - 1);
                    double surfaceAngle = Math.Atan2(point.Y - previousPoint.Y, point.X - previousPoint.X);
                    Assert.AreEqual(angle, Math.PI / 2 - surfaceAngle, epsilon, "Invalid angle for index {0} at distance {1} for resolution {2}", i, distanceToWall, resolution);
                }

                // the second half of the surface should be tilted the other way
                for (int i = reprojectedSurface.Length / 2 + 16; i < reprojectedSurface.Length; i++)
                {
                    Point2D point = reprojectedSurface.GetHitPoint(i);
                    Point2D previousPoint = reprojectedSurface.GetHitPoint(i - 1);
                    double surfaceAngle = Math.Atan2(point.Y - previousPoint.Y, point.X - previousPoint.X);
                    Assert.AreEqual(-angle, Math.PI / 2 - surfaceAngle, epsilon, "Invalid angle for index {0} at distance {1} for resolution {2}", i, distanceToWall, resolution);
                }
            }
        }

        /// <summary>
        /// Validates that the error measure between a global map and an observation is close to 0 when the observation is noise-free.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void MatchNoError_MapTest()
        {
            this.MatchNoError_MapTest(0.01f);
            this.MatchNoError_MapTest(0.02f);
            this.MatchNoError_MapTest(0.1f);
            this.MatchNoError_MapTest(0.2f);
        }

        /// <summary>
        /// Validates that the error measure between a global map and an observation is close to 0 when the observation is noise-free.
        /// </summary>
        /// <param name="resolution">The resolution of the map (size of grid cell in meters)</param>
        public void MatchNoError_MapTest(float resolution)
        {
            Pose2D pose = new Pose2D(0, 128, 0);
            int absoluteWallPosition = 4;

            // create a simple map from an HDP - a wall at 1m
            KinectHorizontalDepthProfile hdp = CreateWallFacingHDP(absoluteWallPosition);
            double precision = resolution / PrecisionFactor;
            double worstCost = (precision * precision) * hdp.Length;
            
            GlobalMap map = new GlobalMap(256, 512, new Pose2D(), resolution);
            IHorizontalDepthProfile reprojectedSurface;
            map.Update(hdp, pose, 2 * absoluteWallPosition, 1);
            ////GlobalMapUtility.ConvertGlobalMapToBitmap(map, @"c:\temp\MatchNoError_MapTest.png");

            double relativeWallPosition = 2;
            hdp = CreateWallFacingHDP(relativeWallPosition);
            pose = new Pose2D((absoluteWallPosition - relativeWallPosition) / resolution, 128, 0);
            double cost = map.Match(hdp, ref pose, true, out reprojectedSurface);
            Assert.IsTrue(Math.Abs(cost) < worstCost, string.Format("Match result {0:N6} is worse than expected {1}. resolution: {2}", cost, worstCost, resolution));
            Assert.AreNotEqual(0, cost, "Zero cost at resolution {0}", resolution);
        }

        /// <summary>
        /// Validates that the error measure between a global map decreases as the position precision increases. 
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void MatchErrorIncreaseX_MapTest()
        {
            this.MatchErrorIncreaseX_MapTest(0.02f, false);
            this.MatchErrorIncreaseX_MapTest(0.04f, false);
            this.MatchErrorIncreaseX_MapTest(0.1f, false);
            this.MatchErrorIncreaseX_MapTest(0.4f, false);
        }

        /// <summary>
        /// Validates that the error measure between a global map decreases as the position precision increases when caching is enabled. 
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void MatchErrorIncreaseXWithMapCache_MapTest()
        {
            this.MatchErrorIncreaseX_MapTest(0.02f, true);
            this.MatchErrorIncreaseX_MapTest(0.04f, true);
            this.MatchErrorIncreaseX_MapTest(0.1f, true);
            this.MatchErrorIncreaseX_MapTest(0.4f, true);
        }

        /// <summary>
        /// Validates that the error measure between a global map decreases as the position precision increases. 
        /// </summary>
        /// <param name="resolution">The resolution of the map (size of grid cell in meters)</param>
        /// <param name="enableMapCache">If true, MapCache is enabled on the global map</param>
        public void MatchErrorIncreaseX_MapTest(float resolution, bool enableMapCache)
        {
            Pose2D pose = new Pose2D(0, 256, 0);
            int absoluteWallPosition = 8;
            int relativeWallPosition = 6;
            int testDistance = absoluteWallPosition - relativeWallPosition;

            // create a simple map from an HDP - a wall at 2m
            KinectHorizontalDepthProfile hdp = CreateWallFacingHDP(absoluteWallPosition);
            GlobalMap map = new GlobalMap(512, 512, new Pose2D(), resolution);
            if (enableMapCache)
            {
                map.EnableCaching();
            }

            IHorizontalDepthProfile reprojectedSurface;
            map.Update(hdp, pose, 2 * absoluteWallPosition, 1);

            // expected reading from 2m
            hdp = CreateWallFacingHDP(relativeWallPosition);

            ////GlobalMapUtility.ConvertGlobalMapToBitmap(map, @"c:\temp\MatchErrorIncreaseX_MapTest.png");

            // start away from the true pose and move closer to the correct pose in 1mm increments.
            double frontCost = double.MinValue;
            double behindCost = double.MinValue;

            // we expect a 20 to 1 ratio of resolution to accuracy, e.g. 1mm accuracy when using 2cm grid cells
            double increment = resolution / PrecisionFactor;
            for (double d = 0.4; d > 0; d -= increment)
            {
                // verify in front of the true pose
                pose = new Pose2D((testDistance + d) / resolution, 256, 0);
                double newCost = map.Match(hdp, ref pose, true, out reprojectedSurface);
                Assert.AreEqual(relativeWallPosition - d, reprojectedSurface.GetHitPoint(320).X, MathConstants.ErrorEpsilon, "Distance {0}. Resolution: {1}", d, resolution);
                Assert.IsTrue(Math.Abs(newCost) < Math.Abs(frontCost), string.Format("Match cost at {0} in front of the true pose didn't improve as expected: {1}, {2}. Resolution: {3}", d, newCost, frontCost, resolution));
                Assert.AreNotEqual(0, newCost, "Zero cost at distace {0} ({1}). Resolution {2}", d, pose.X, resolution);
                frontCost = newCost;

                // verify behind the true pose
                pose = new Pose2D((testDistance - d) / resolution, 256, 0);
                newCost = map.Match(hdp, ref pose, true, out reprojectedSurface);
                Assert.IsTrue(Math.Abs(newCost) < Math.Abs(behindCost), string.Format("Match cost at {0} behind the true pose didn't improve as expected: {1}, {2}. Resolution: {3}", d, newCost, behindCost, resolution));
                Assert.AreNotEqual(0, newCost, "Zero cost at distace {0} ({1}). Resolution {2}", d, pose.X, resolution);
                behindCost = newCost;
            }
        }

        /// <summary>
        /// Validates that the error measure between a global map decreases as the position precision increases. 
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void MatchErrorIncreaseY_MapTest()
        {
            this.MatchErrorIncreaseY_MapTest(0.02f, false);
            this.MatchErrorIncreaseY_MapTest(0.04f, false);
            this.MatchErrorIncreaseY_MapTest(0.1f, false);
            this.MatchErrorIncreaseY_MapTest(0.2f, false);
        }

        /// <summary>
        /// Validates that the error measure between a global map decreases as the position precision increases. 
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void MatchErrorIncreaseYWithMapCache_MapTest()
        {
            this.MatchErrorIncreaseY_MapTest(0.02f, true);
            this.MatchErrorIncreaseY_MapTest(0.04f, true);
            this.MatchErrorIncreaseY_MapTest(0.1f, true);
            this.MatchErrorIncreaseY_MapTest(0.2f, true);
        }

        /// <summary>
        /// Validates that the error measure between a global map decreases as the position precision increases. 
        /// </summary>
        /// <param name="resolution">The resolution of the map (size of grid cell in meters)</param>
        /// <param name="enableMapCache">If true, MapCache is enabled on the global map</param>
        public void MatchErrorIncreaseY_MapTest(float resolution, bool enableMapCache)
        {
            Pose2D pose = new Pose2D(256, 0, Math.PI / 2);
            int absoluteWallPosition = 8;
            int relativeWallPosition = 6;
            int testDistance = absoluteWallPosition - relativeWallPosition;

            // create a simple map from an HDP - a wall at 2m
            KinectHorizontalDepthProfile hdp = CreateWallFacingHDP(absoluteWallPosition);
            GlobalMap map = new GlobalMap(512, 512, new Pose2D(), resolution);
            if (enableMapCache)
            {
                map.EnableCaching();
            }

            IHorizontalDepthProfile reprojectedSurface;
            map.Update(hdp, pose, 2 * absoluteWallPosition, 1);

            // expected reading from 2m
            hdp = CreateWallFacingHDP(relativeWallPosition);

            ////GlobalMapUtility.ConvertGlobalMapToBitmap(map, @"c:\temp\MatchErrorIncreaseY_MapTest.png");

            // start away from the true pose and move closer to the correct pose in 1mm increments.
            double frontCost = double.MinValue;
            double behindCost = double.MinValue;

            // we expect a 20 to 1 ratio of resolution to accuracy, e.g. 1mm accuracy when using 2cm grid cells
            double increment = resolution / PrecisionFactor;
            for (double d = 0.4; d > 0; d -= increment)
            {
                // verify in front of the true pose
                pose = new Pose2D(256, (testDistance + d) / resolution, Math.PI / 2);
                double newCost = map.Match(hdp, ref pose, true, out reprojectedSurface);
                Assert.IsTrue(Math.Abs(newCost) < Math.Abs(frontCost), string.Format("Match cost at {0} in front of the true pose didn't improve as expected: {1}, {2}. Resolution: {3}", d, newCost, frontCost, resolution));
                Assert.AreNotEqual(0, newCost, "Zero cost at distace {0} ({1}). Resolution {2}", d, pose.X, resolution);
                frontCost = newCost;

                // verify behind the true pose
                pose = new Pose2D(256, (testDistance - d) / resolution, Math.PI / 2);
                newCost = map.Match(hdp, ref pose, true, out reprojectedSurface);
                Assert.IsTrue(Math.Abs(newCost) < Math.Abs(behindCost), string.Format("Match cost at {0} behind the true pose didn't improve as expected: {1}, {2}. Resolution: {3}", d, newCost, behindCost, resolution));
                Assert.AreNotEqual(0, newCost, "Zero cost at distace {0} ({1}). Resolution {2}", d, pose.X, resolution);
                behindCost = newCost;
            }
        }

        /// <summary>
        /// Validates that the error measure between a global map decreases as the position precision increases. 
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void MatchErrorIncreaseHeading_MapTest()
        {
            this.MatchErrorIncreaseHeading_MapTest(0.02f, false);
            this.MatchErrorIncreaseHeading_MapTest(0.04f, false);
            this.MatchErrorIncreaseHeading_MapTest(0.1f, false);
            this.MatchErrorIncreaseHeading_MapTest(0.2f, false);
        }

        /// <summary>
        /// Validates that the error measure between a global map decreases as the position precision increases. 
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void MatchErrorIncreaseHeadingWithMapCache_MapTest()
        {
            this.MatchErrorIncreaseHeading_MapTest(0.02f, true);
            this.MatchErrorIncreaseHeading_MapTest(0.04f, true);
            this.MatchErrorIncreaseHeading_MapTest(0.1f, true);
            this.MatchErrorIncreaseHeading_MapTest(0.2f, true);
        }

        /// <summary>
        /// Validates that the error measure between a global map decreases as the position precision increases. 
        /// </summary>
        /// <param name="resolution">The resolution of the map (size of grid cell in meters)</param>
        /// <param name="enableMapCache">If true, MapCache is enabled on the global map</param>
        public void MatchErrorIncreaseHeading_MapTest(float resolution, bool enableMapCache)
        {
            Pose2D pose = new Pose2D(0, 256, 0);
            int absoluteWallPosition = 8;
            int relativeWallPosition = 4;
            int testDistance = absoluteWallPosition - relativeWallPosition;

            // create a simple map from an HDP - a wall at 5m
            KinectHorizontalDepthProfile hdp = CreateWallFacingHDP(absoluteWallPosition);
            GlobalMap map = new GlobalMap(512, 512, new Pose2D(), resolution);
            if (enableMapCache)
            {
                map.EnableCaching();
            }

            IHorizontalDepthProfile reprojectedSurface;
            map.Update(hdp, pose, 2 * absoluteWallPosition, 1);
            ////GlobalMapUtility.ConvertGlobalMapToBitmap(map, @"c:\temp\MatchErrorIncreaseHeading_MapTest.png");

            // start away from the true pose and move closer to the correct pose in 1mm increments.
            double leftCost = double.MinValue;
            double rightCost = double.MinValue;
            hdp = CreateWallFacingHDP(relativeWallPosition);
            double increment = MathConstants.Degrees2Radians / 10;

            // decrease the angle in small increments approaching 0.
            for (double a = 15 * MathConstants.Degrees2Radians; a >= 0; a -= increment)
            {
                // verify a rotation left of the true pose
                pose = new Pose2D(testDistance / resolution, 256, a);
                double newCost = map.Match(hdp, ref pose, true, out reprojectedSurface);
                Assert.IsTrue(Math.Abs(newCost) < Math.Abs(leftCost), string.Format("Match cost to the left of the true pose ({0} rad) didn't improve as expected: {1}, {2}. Resolution {3}", a, newCost, leftCost, resolution));
                Assert.AreNotEqual(0, newCost, "Zero cost at angle {0}. Resolution {1}", a, resolution);
                leftCost = newCost;

                // verify a rotation right of the true pose
                pose = new Pose2D(testDistance / resolution, 256, -a);
                newCost = map.Match(hdp, ref pose, true, out reprojectedSurface);
                Assert.IsTrue(Math.Abs(newCost) < Math.Abs(rightCost), string.Format("Match cost to the right of the true pose ({0} rad) didn't improve as expected: {1}, {2}. Resolution {3}", a, newCost, rightCost, resolution));
                Assert.AreNotEqual(0, newCost, "Zero cost at angle {0}. Resolution {1}", a, resolution);
                rightCost = newCost;
            }

            // check that 0 orientation is almost as good. Use of normals in the cost function skews the result close to 0 rad, but the cost is very small anyway
            pose = new Pose2D(testDistance / resolution, 256, 0);
            double precision = resolution / PrecisionFactor;
            double worstCost = (precision * precision) * hdp.Length;
            double zeroCost = map.Match(hdp, ref pose, true, out reprojectedSurface);
            Assert.IsTrue(Math.Abs(zeroCost) < worstCost, string.Format("Match at the true pose (exactly 0 rad) is not near 0 as expected: {0}", zeroCost));
            Assert.AreNotEqual(0, zeroCost);
        }

        /// <summary>
        /// Validates that the error measure between a global map and an observation is close to 0 when the observation is noise-free, even when reusing entries from the map cache.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void MatchWithMapCache_MapTest()
        {
            this.MatchWithMapCache_MapTest(0.02f);
            this.MatchWithMapCache_MapTest(0.04f);
            this.MatchWithMapCache_MapTest(0.1f);
            this.MatchWithMapCache_MapTest(0.2f);
        }

        /// <summary>
        /// Validates that the error measure between a global map and an observation is close to 0 when the observation is noise-free, even when reusing entries from the map cache.
        /// </summary>
        /// <param name="resolution">The resolution of the map (size of grid cell in meters)</param>
        public void MatchWithMapCache_MapTest(float resolution)
        {
            Pose2D pose = new Pose2D(0, 256, 0);
            double absoluteWallPosition = 6;

            // create a simple map from an HDP - a wall at 1m
            KinectHorizontalDepthProfile hdp = CreateWallFacingHDP(absoluteWallPosition);
            GlobalMap map = new GlobalMap(512, 512, new Pose2D(), resolution);
            map.EnableCaching();
            IHorizontalDepthProfile reprojectedSurface;
            map.Update(hdp, pose, 2 * absoluteWallPosition, 1);
            ////GlobalMapUtility.ConvertGlobalMapToBitmap(map, @"c:\temp\MatchWithMapCache_MapTest.png");

            double relativeWallPosition = 2.79; // some random pose
            hdp = CreateWallFacingHDP(relativeWallPosition);
            double precision = resolution / PrecisionFactor;
            double worstPossibleCostForPerfectMatch = (precision * precision) * hdp.Length;
            pose = new Pose2D((absoluteWallPosition - relativeWallPosition) / resolution, 256, 0);
            double cost = map.Match(hdp, ref pose, true, out reprojectedSurface);
            Assert.IsTrue(Math.Abs(cost) < worstPossibleCostForPerfectMatch, string.Format("Match result {0:N6} is worse than the worst cost expected {1}", cost, worstPossibleCostForPerfectMatch));
            
            // do it again, this time it should be a cache hit
            double secondCost = map.Match(hdp, ref pose, true, out reprojectedSurface);
            Assert.AreEqual(cost, secondCost, string.Format("Match result {0:N4} after caching is different than before {1:N4}", secondCost, cost));

            // should still work from a slightly different position, with the same cache hit 
            relativeWallPosition = relativeWallPosition - 0.1 * resolution;
            hdp = CreateWallFacingHDP(relativeWallPosition);
            pose = new Pose2D((absoluteWallPosition - relativeWallPosition) / resolution, 256, 0);
            cost = map.Match(hdp, ref pose, true, out reprojectedSurface);
            Assert.IsTrue(Math.Abs(cost) < worstPossibleCostForPerfectMatch, string.Format("Match result {0:N6} is worse than the worst cost expected {1}", cost, worstPossibleCostForPerfectMatch));

            // clear the map cache and compute the fitness again
            // last cost should be the same as the cache-free one (i.e. after the cache is reset)
            map.EnableCaching();
            secondCost = map.Match(hdp, ref pose, true, out reprojectedSurface);
            Assert.AreEqual(cost, secondCost, MathConstants.ErrorEpsilon, string.Format("Match result {0:N4} without caching is different than before {1:N4}", secondCost, cost));
        }

        /// <summary>
        /// Generates an HDP that simulates the camera facing a wall at the given distance
        /// </summary>
        /// <param name="distance">Distance to the wall, in meters</param>
        /// <returns>The HDP.</returns>
        private static KinectHorizontalDepthProfile CreateWallFacingHDP(double distance)
        {
            // create camera intrinsics with no dead pixels
            DepthCameraIntrinsicParameters intrinsicParams = new DepthCameraIntrinsicParameters(
                DepthCameraIntrinsicParameters.KinectNominalFocalLengthInPixels, 
                DepthCameraIntrinsicParameters.KinectNominalFocalLengthInPixels, 
                DepthCameraIntrinsicParameters.KinectHorizontalResolution / 2,
                DepthCameraIntrinsicParameters.KinectVerticalResolution / 2,
                DepthCameraIntrinsicParameters.KinectHorizontalResolution,
                DepthCameraIntrinsicParameters.KinectVerticalResolution,
                0);

            short[] depths = new short[640];
            for (int i = 0; i < depths.Length; i++)
            {
                depths[i] = (short)(distance * 1000);
            }

            return new KinectHorizontalDepthProfile(depths, intrinsicParams);
        }
    }
}
